#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "mqtt_client.h"
#include "esp_event.h"
#include "esp_wifi.h"
#include "nvs_flash.h"
#include "cJSON.h"

#include "sdkconfig.h"
#include "freertos/queue.h"
#include "freertos/event_groups.h"
#include "esp_system.h"
#include "esp_log.h"
#include <sys/time.h>
#include "esp_sntp.h"

#include "sntp_time.h"
#include "wifi_mqtt.h"
#include "uart.h"
#include "SerialServo.h"
#include "driver/gpio.h"

extern alarm_threshold threshold; // 声明全局变量
int servo_value[6] = {2,487,937,841,814,500}; //舵机位置存储值
extern int position_array[5];
extern int temperature_array[5];
extern int volatge_array[5];
extern bool mqtt_connect_flag;
extern esp_mqtt_client_handle_t emcht;
char send_message[100]="";
int step_on = 0;
extern char* state_p;


void pub_task(void){

        if(mqtt_connect_flag){

                position_array[0] = (int)(((float)(position_array[0]-500))/4.17)*(-1);
                sprintf(send_message, "%d", position_array[0]);
                esp_mqtt_client_publish(emcht,"unity_P1",send_message,strlen(send_message),1,0);
                memset(send_message, 0, sizeof(send_message));
                
                // printf("position_array[1]=%d\r\n",position_array[0]);
                //P2
                position_array[1] = (int)(((float)(position_array[1]-500))/4.17)*(-1);
                sprintf(send_message, "%d", position_array[1]);
                esp_mqtt_client_publish(emcht,"unity_P2",send_message,strlen(send_message),1,0);
                memset(send_message, 0, sizeof(send_message));
                
                // printf("position_array[2]=%d\r\n",position_array[1]);
            
                //P3
                position_array[2] = (int)(((float)(position_array[2]-500))/4.17);
                sprintf(send_message, "%d", position_array[2]);
                esp_mqtt_client_publish(emcht,"unity_P3",send_message,strlen(send_message),1,0);
                memset(send_message, 0, sizeof(send_message));
                
                // printf("position_array[3]=%d\r\n",position_array[2]);
            
                //P4
                position_array[3] = (int)(((float)(position_array[3]-500))/4.17);
                sprintf(send_message, "%d", position_array[3]);
                esp_mqtt_client_publish(emcht,"unity_P4",send_message,strlen(send_message),1,0);
                memset(send_message, 0, sizeof(send_message));
                

                sprintf(send_message, "%d", temperature_array[0]);
                esp_mqtt_client_publish(emcht,"unity_st1",send_message,strlen(send_message),1,0);
                memset(send_message, 0, sizeof(send_message));
                
                //printf("temperature_array[0]=%d\r\n",temperature_array[0]);
            
                sprintf(send_message, "%d", temperature_array[1]);
                esp_mqtt_client_publish(emcht,"unity_st2",send_message,strlen(send_message),1,0);
                memset(send_message, 0, sizeof(send_message));
                
                //printf("temperature_array[1]=%d\r\n",temperature_array[1]);
            
                sprintf(send_message, "%d", temperature_array[2]);
                esp_mqtt_client_publish(emcht,"unity_st3",send_message,strlen(send_message),1,0);
                memset(send_message, 0, sizeof(send_message));
                
                //printf("temperature_array[2]=%d\r\n",temperature_array[2]);
            
                sprintf(send_message, "%d", temperature_array[3]);
                esp_mqtt_client_publish(emcht,"unity_st4",send_message,strlen(send_message),1,0);
                memset(send_message, 0, sizeof(send_message));
                
                //printf("temperature_array[3]=%d\r\n",temperature_array[3]);
            
                sprintf(send_message, "%d", temperature_array[4]);
                esp_mqtt_client_publish(emcht,"unity_st5",send_message,strlen(send_message),1,0);
                memset(send_message, 0, sizeof(send_message));
                

                sprintf(send_message, "%.03f", (float)volatge_array[0]*0.001);
                esp_mqtt_client_publish(emcht,"unity_sv1",send_message,strlen(send_message),1,0);
                memset(send_message, 0, sizeof(send_message));
                
            
                sprintf(send_message, "%.03f", volatge_array[1]*0.001);
                esp_mqtt_client_publish(emcht,"unity_sv2",send_message,strlen(send_message),1,0);
                memset(send_message, 0, sizeof(send_message));
               
            
                sprintf(send_message, "%.03f", volatge_array[2]*0.001);
                esp_mqtt_client_publish(emcht,"unity_sv3",send_message,strlen(send_message),1,0);
                memset(send_message, 0, sizeof(send_message));
               
            
                sprintf(send_message, "%.03f", volatge_array[3]*0.001);
                esp_mqtt_client_publish(emcht,"unity_sv4",send_message,strlen(send_message),1,0);
                memset(send_message, 0, sizeof(send_message));

                sprintf(send_message, "%.03f", volatge_array[4]*0.001);
                esp_mqtt_client_publish(emcht,"unity_sv5",send_message,strlen(send_message),1,0);
                memset(send_message, 0, sizeof(send_message));
           
            
                sprintf(send_message, "%.01f - %.01f", threshold.voltage_low, threshold.voltage_high);
                esp_mqtt_client_publish(emcht,"voltage_limit",send_message,strlen(send_message),1,0);
                memset(send_message, 0, sizeof(send_message));
                


                sprintf(send_message, "> %d", threshold.temp_limit);
                esp_mqtt_client_publish(emcht,"temp_limit",send_message,strlen(send_message),1,0);
                memset(send_message, 0, sizeof(send_message));
                
                //比较阈值判断for循环比较
                for(int i=0;i<5;i++)
                {
                    if(volatge_array[i]*0.001<threshold.voltage_low)
                    {
                        sprintf(send_message, "欠压");
                        esp_mqtt_client_publish(emcht,"alarm",send_message,strlen("欠压"),1,0);
                        memset(send_message, 0, sizeof(send_message));
                        gpio_set_level(39, 1);
                        state_p = "warning";
                    }
                    else if(volatge_array[i]*0.001>threshold.voltage_high)
                    {
                        sprintf(send_message, "过压");
                        esp_mqtt_client_publish(emcht,"alarm",send_message,strlen("过压"),1,0);
                        memset(send_message, 0, sizeof(send_message));
                        gpio_set_level(39, 1);
                        state_p = "warning";
                    }
                    else if(temperature_array[i]>threshold.temp_limit)
                    {
                        sprintf(send_message, "过热");
                        esp_mqtt_client_publish(emcht,"alarm",send_message,strlen("过热"),1,0);
                        memset(send_message, 0, sizeof(send_message));
                        gpio_set_level(39, 1);
                        state_p = "warning";
                    }
                    else
                    {
                        sprintf(send_message, "正常");
                        esp_mqtt_client_publish(emcht,"alarm",send_message,strlen("正常"),1,0);
                        memset(send_message, 0, sizeof(send_message));
                        gpio_set_level(39, 0);
                        state_p = "normal";
                    }
                }

    }
}

void print_task(void){


        for (int servo_id = 1; servo_id < 6; servo_id++)
        {
            printf("S%d_position:%d\r\n",servo_id,position_array[servo_id-1]);
            printf("S%d_vin:%.03f\r\n",servo_id,(float)volatge_array[servo_id-1]*0.001);
            printf("S%d_temp:%d\r\n",servo_id,temperature_array[servo_id-1]);
        }
        printf("servo_mode=%d\r\n",servo_value[0]);
        vTaskDelay(20/portTICK_PERIOD_MS);
    
}

void uart_task(void){
    int count=0;
    // do
    // {
    //     for(int read_id=0;read_id<5;read_id++)
    //     {
    //         LobotSerialServoReadPosition(UART1_UX, read_id+1);
    //         vTaskDelay(10/portTICK_PERIOD_MS);
    //         LobotSerialServoReadTemp(UART1_UX, read_id+1);
    //         vTaskDelay(10/portTICK_PERIOD_MS);
    //         LobotSerialServoReadVin(UART1_UX, read_id+1);
    //         vTaskDelay(10/portTICK_PERIOD_MS);
    //     }
    // } while (0);

    while(1){


        if(1)
        {
            if(step_on == 2)
            {
                gpio_set_level(38, 1);//使能
    
                vTaskDelay(100/portTICK_PERIOD_MS);
                gpio_set_level(37, 0);//方向向右
                //步进电机驱动程序
                  for(int motor_i=0;motor_i<200*3;motor_i++)
                  {
                      gpio_set_level(36, 1);
                      vTaskDelay(10/portTICK_PERIOD_MS);
                      gpio_set_level(36, 0);
                      vTaskDelay(10/portTICK_PERIOD_MS);
                      if(step_on == 0){goto end_step;}
                  }
    
            }
            if(step_on == 1)
            {
                gpio_set_level(38, 1);//使能
                  vTaskDelay(100/portTICK_PERIOD_MS);
                  gpio_set_level(37, 1);//方向向左
                  //步进电机驱动程序
                    for(int motor_i=0;motor_i<200*3;motor_i++)
                    {
                        gpio_set_level(36, 1);
                        vTaskDelay(10/portTICK_PERIOD_MS);
                        gpio_set_level(36, 0);
                        vTaskDelay(10/portTICK_PERIOD_MS);
                        if(step_on == 0){goto end_step;}
                    }
            }
            end_step:    
            gpio_set_level(38, 0);//失能

        }


        count++;
        servo_data_read();
        // print_task();
        pub_task();

        if(servo_value[0] == 2)
        {
        LobotSerialServoUnload();
        vTaskDelay(10/portTICK_PERIOD_MS);
        if(count==20)
        {
            sntp_task();
            count = 0;
        }
        }
        if(servo_value[0] == 1)
        {
            printf("servo_1=%d\r\n",servo_value[1]);
            printf("servo_2=%d\r\n",servo_value[2]);
            printf("servo_3=%d\r\n",servo_value[3]);
            printf("servo_4=%d\r\n",servo_value[4]);
            printf("servo_5=%d\r\n",servo_value[5]);

        LobotSerialServoMove(UART1_UX,1,servo_value[1],2000);
        LobotSerialServoMove(UART1_UX,2,servo_value[2],2000);
        LobotSerialServoMove(UART1_UX,3,servo_value[3],2000);
        LobotSerialServoMove(UART1_UX,4,servo_value[4],2000);
        LobotSerialServoMove(UART1_UX,5,servo_value[5],2000);
        vTaskDelay(2000/portTICK_PERIOD_MS);
        if(count > 2)
        {
            sntp_task();
            count = 0;
        }
         }
    }

}

void init_(void)
{
    WiFi_Init();

    esp_initialize_sntp();
    // 延时等待SNTP初始化完成
    do {
        vTaskDelay(100 / portTICK_PERIOD_MS);
    } while (sntp_get_sync_status() == SNTP_SYNC_STATUS_RESET);

    // 成功获取网络时间后停止NTP请求，不然设备重启后会造成获取网络时间失败的现象
    // 大概是服务器时根据心跳时间来删除客户端的，如果不是stop结束的客户端，下次连接服务器时就会出错
	esp_sntp_stop();

    uart0_1_init(115200);
    vTaskDelay(100/portTICK_PERIOD_MS);


              gpio_config_t io_conf = {};
              io_conf.intr_type = GPIO_INTR_DISABLE;
              io_conf.mode = GPIO_MODE_OUTPUT;
              io_conf.pin_bit_mask = ((1ULL<<GPIO_NUM_36) | (1ULL<<GPIO_NUM_37) | \
              (1ULL<<GPIO_NUM_38) | (1ULL<<GPIO_NUM_39));
              io_conf.pull_down_en = 0;
              io_conf.pull_up_en = 0;
           
              gpio_config(&io_conf);
              gpio_set_direction(36, GPIO_MODE_OUTPUT);//模拟PWM
              gpio_set_direction(37, GPIO_MODE_OUTPUT);//方向引脚
              gpio_set_direction(38, GPIO_MODE_OUTPUT);//低电平使能
              gpio_set_direction(39, GPIO_MODE_OUTPUT);//低电平使能
              gpio_set_level(38, 0);//失能

            //   gpio_set_level(39, 1);//继电器驱动
            //   vTaskDelay(10000/portTICK_PERIOD_MS);
            //   gpio_set_level(39, 0);//继电器驱动
    // for(int action_time=0;action_time<2;action_time++)
    // {
    //     LobotSerialServoMove(UART1_UX,1,487,2000);//加大逆时针旋转从上看舵盘
    //     LobotSerialServoMove(UART1_UX,2,500,2000);
    //     LobotSerialServoMove(UART1_UX,3,500,2000);
    //     LobotSerialServoMove(UART1_UX,4,500,2000);
    //     LobotSerialServoMove(UART1_UX,5,500,2000);
    //     vTaskDelay(3500/portTICK_PERIOD_MS);
        
    //     LobotSerialServoMove(UART1_UX,1,487,2000);//加大逆时针旋转从上看舵盘
    //     LobotSerialServoMove(UART1_UX,2,937,2000);
    //     LobotSerialServoMove(UART1_UX,3,841,2000);
    //     LobotSerialServoMove(UART1_UX,4,814,2000);
    //     LobotSerialServoMove(UART1_UX,5,500,2000);
    //     vTaskDelay(3500/portTICK_PERIOD_MS);
    // }
    LobotSerialServoUnload();

    
    //创建普通优先级任务
    //xTaskCreate(uart_task, "uart_task", 1024*2, NULL,4, NULL);
    //xTaskCreate(pub_task, "pub_task", 1024*2, NULL, 3, NULL);
    //xTaskCreate(sntp_task, "sntp_task", 1024*2, NULL, 3, NULL);


    uart_task();

}
void app_main(void){
    init_();
}



